Прямая задача кинематики SCARA

from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook

from kinematics import Vector, Quaternion, Transform
import graphics

Решение прямой задачи кинематики для манипулятора SCARA

Манипулятор кинематической схемы SCARA обладает четыремя степенями подвижности.

Можно выделить высоту колонны, длины первого и второго звеньев.

Обобщенные координаты будут в радианах и метрах:

Обобщенная координата	Обозначение	Размерность
Вращение вокруг колонны	$q_0$	радиан
Вращение в локте	$q_1$	радиан
Вращение инструмента	$q_2$	радиан
Перемещение инструмента	$q_3$	метр

def scara_chain(q, l, math_source=np):
    base = Transform.identity()
    column = base + Transform(
        Vector(0, 0, l[0]),
        Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source)
    )
    elbow = column + Transform(
        Vector(l[1], 0, 0),
        Quaternion.from_angle_axis(q[1], Vector(0, 0, 1), math_source)
    )
    tool = elbow + Transform(
        Vector(l[2], 0, 0),
        Quaternion.from_angle_axis(q[2], Vector(0, 0, 1), math_source)
    )
    flange = tool + Transform(
        Vector(0, 0, -q[3]),
        Quaternion.identity()
    )
    return [
        base,
        column,
        elbow,
        tool,
        flange
    ]

Зададим закон изменения обобщенных координат:

def scara_q(t, total):
    #omega = t / total * np.pi * 2
    a = (t - total) / 2
    omega = (-np.cos(3.14 / total * t) + 1.0)*5
    return [
        np.pi / 2 * np.sin(omega)-np.pi,
        np.pi / 2*np.cos(-omega),
        omega,
        3 + 3 * np.cos(omega)
    ]

Укажем длины звеньев:

scara_l = [8, 4, 3]

scara_fig = plt.figure()
ax = scara_fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)

total = 100

def animate(frame):
    chain = scara_chain(scara_q(frame, total), scara_l)
    (x, y, z) = graphics.chain_to_points(chain)
    lines.set_data_3d(x, y, z)
    global r, g, b
    r.remove(); g.remove(); b.remove()
    r, g, b = graphics.axis(ax, chain[-1], 0.5)

    
animate(0)
fps = 25
scara_ani = animation.FuncAnimation(
    scara_fig,
    animate,
    frames=total,
    interval=1000.0/fps
)

HTML(scara_ani.to_jshtml())

Once Loop Reflect

Отображение траектории

t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
x, y, z = graphics.chain_to_points(chain)

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
ax.plot(x[2], y[2], z[2], color="#c0c0c0")
ax.plot(x[3], y[3], z[3], color="#c0c0c0")
ax.plot(x[4], y[4], z[4], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
fig.show()

Определим диапазон изменения $Z$ координаты фланца:

print(
    "X координата менялась в диапазоне от",
    np.min(x[-1]),
    "до",
    np.max(x[-1]),
        "\nY координата менялась в диапазоне от",
    np.min(y[-1]),
    "до",
    np.max(y[-1]),
        "\nZ координата менялась в диапазоне от",
    np.min(z[-1]),
    "до",
    np.max(z[-1])
)

X координата менялась в диапазоне от -5.350516053106826 до 0.9129775104309787 
Y координата менялась в диапазоне от -6.9999506267887295 до 6.99999478548686 
Z координата менялась в диапазоне от 2.0 до 7.9999997759276225

Измерение скорости движения

Численно продиффиринцируем для всех точек траектории:

t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
end = chain[-1]
velocity_x = np.diff(end.translation.x) / step
velocity_y = np.diff(end.translation.y) / step
velocity_z = np.diff(end.translation.z) / step
velocity_total = (velocity_x ** 2 + velocity_y ** 2 + velocity_z ** 2) ** 0.5

fig = plt.figure()
ax = fig.add_subplot()

ax.plot(t[:-1], velocity_x, color="#ff0000", label="$V_x$")
ax.plot(t[:-1], velocity_y, color="#00ff00", label="$V_y$")
ax.plot(t[:-1], velocity_z, color="#0000ff", label="$V_z$")
ax.plot(t[:-1], velocity_total, color="#000000", label="$V$")
fig.legend()
fig.show()

Оценим максимальную скорость на траектории:

index_max = np.argmax(velocity_total)
vx_max = np.argmax(velocity_x)
vy_max = np.argmax(velocity_y)
vz_max = np.argmax(velocity_z)
print(
    "Максимальная скорость была",    velocity_total[index_max],    "в момент",    index_max * step,    "сек\n",    
    "Максимальная скорость по оси X была",    velocity_x[vx_max],    "в момент",    vx_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_y[vy_max],    "в момент",    vy_max * step,    "сек\n",
    "Максимальная скорость по оси Y была",    velocity_z[vz_max],    "в момент",    vz_max * step,    "сек\n"
)

Максимальная скорость была 15.227431344661596 в момент 5.3 сек
 Максимальная скорость по оси X была 8.333183113728992 в момент 4.03 сек
 Максимальная скорость по оси Y была 12.771053653587439 в момент 3.41 сек
 Максимальная скорость по оси Y была 3.917259906850923 в момент 6.82 сек

Аналитическое решение прямой задачи кинематики

import sympy as sp

l_0, l_1, l_2 = sp.symbols("l_0, l_1, l_2")
q_0, q_1, q_2, q_3 = sp.symbols("q_0, q_1, q_2, q_3")

flange = scara_chain([q_0, q_1, q_2, q_3], [l_0, l_1, l_2], sp)[-1]
sp.simplify(flange.translation.x)

$\displaystyle - l_{1} \sin^{2}{\left(\frac{q_{0}}{2} \right)} + l_{1} \cos^{2}{\left(\frac{q_{0}}{2} \right)} - l_{2} \sin^{2}{\left(\frac{q_{0}}{2} + \frac{q_{1}}{2} \right)} + l_{2} \cos^{2}{\left(\frac{q_{0}}{2} + \frac{q_{1}}{2} \right)}$

sp.simplify(flange.translation.y)

$\displaystyle l_{1} \sin{\left(q_{0} \right)} + l_{2} \sin{\left(q_{0} + q_{1} \right)}$

sp.simplify(flange.translation.z)

$\displaystyle l_{0} - q_{3}$

Самостоятельные задания

Для своего закона изменения обобщенных координат

• Оцените диапазон $X$, $Y$ и $Z$ координат для данной траектории;
• Оцените максимальную скорость по осям;
• Оцените абсолютную максимальную скорость.